#include <ros/ros.h>
#include "executor/chassis_executor.h"
#include "executor/gimbal_executor.h"
#include "behavior_tree/behavior_node.h"
#include "behavior_tree/behavior_tree.h"
#include "example_behavior/back_boot_area_behavior.h"
#include "example_behavior/escape_behavior.h"
#include "example_behavior/chase_behavior.h"
#include "example_behavior/search_behavior.h"
#include "example_behavior/patrol_behavior.h"
#include "example_behavior/goal_behavior.h"
#include "example_behavior/circle_behavior.h"
#include "example_behavior/shake_behavior.h"
#include "example_behavior/dance_behavior.h"
#include <iostream>

ros::Time seqpass;
//ros::Time tmix;

void param_init(ros::NodeHandle& p){

}

int main(int argc, char **argv){
    ros::init(argc, argv, "behavior_test_node");
    std::string full_path = ros::package::getPath("roborts_decision") + "/config/decision.prototxt";
   ros::NodeHandle nh_p("~") ;
    param_init(nh_p);
    auto blackboard_ptr_ = std::make_shared<roborts_decision::Blackboard>(full_path);
    auto chassis_executor_ptr_ = std::make_shared<roborts_decision::ChassisExecutor>();
    auto gimbal_executor_ptr_ = std::make_shared<roborts_decision::GimbalExecutor>();
    //auto blackboard = new roborts_decision::Blackboard(full_path);

    //roborts_decision::DanceBehavior dance_behavior(chassis_executor,  gimbal_executor, blackboard, full_path);
    //roborts_decision::ShakeBehavior shake_behavior(gimbal_executor, blackboard, full_path);

    auto dance_action_=std::make_shared<roborts_decision::DanceAction>(blackboard_ptr_, chassis_executor_ptr_, gimbal_executor_ptr_);
    auto shake_action_=std::make_shared<roborts_decision::ShakeAction>(blackboard_ptr_, gimbal_executor_ptr_);
    auto patrol_action_ = std::make_shared<roborts_decision::PatrolAction>(blackboard_ptr_, chassis_executor_ptr_);

    auto patrol_parallel_=std::make_shared<roborts_decision::ParallelNode>("patrol",blackboard_ptr_,1);
     patrol_parallel_->AddChildren(shake_action_);
     patrol_parallel_->AddChildren(patrol_action_);

    auto dance_condition_=std::make_shared<roborts_decision::PreconditionNode>("dance condition", blackboard_ptr_, [&]()
    {
        if (blackboard_ptr_->GetIfEnemy().data==1)
        {
                seqpass=blackboard_ptr_->Getseq();
            //chassis_executor->Cancel();
            return true;}
        else 
        {
            if (ros::Time::now()-seqpass>ros::Duration(1.0))
            {
                // seqpass=tmix;
                 return false;
            }
            else return true;
        }
    }, roborts_decision::AbortType::BOTH);

    dance_condition_->SetChild(dance_action_);

    auto root_selector_ = std::make_shared<roborts_decision::SelectorNode>("dance or not",blackboard_ptr_);
    root_selector_->AddChildren(dance_condition_);
    root_selector_->AddChildren(patrol_parallel_);
    roborts_decision::BehaviorTree root(root_selector_,50);
    root.Run();

    return 0;
}